#include "fangshe_sm/state_machine.hpp"
#include <rclcpp/parameter_client.hpp>
#include <rclcpp/parameter.hpp>
#include <lifecycle_msgs/srv/change_state.hpp>
#include <lifecycle_msgs/srv/get_state.hpp>
#include <rcl_interfaces/srv/set_parameters.hpp>
#include <rcl_interfaces/msg/parameter.hpp>
#include <rcl_interfaces/msg/parameter_value.hpp>
#include <chrono>
#include <thread>
#include <functional>
#include <algorithm>
#include <string>
#include <sstream> 
#include <utility>

namespace fangshe_sm {

    StateMachine::StateMachine(const rclcpp::NodeOptions & options)
    : rclcpp::Node("state_machine", options)
    {
        RCLCPP_INFO(this->get_logger(), "Creating State Machine Node");

        // Declare and load parameters
        declare_parameters();
        load_parameters();

        // Create lifecycle and parameter clients
        create_lifecycle_clients();
        create_parameter_clients();

        // Initialize state machine
        RCLCPP_INFO(this->get_logger(), "Starting state machine logic...");
        // Start the main state machine loop using a timer
        this->create_wall_timer(std::chrono::milliseconds(100), // Run logic every 100ms
            std::bind(&StateMachine::state_machine_logic, this));
    }

    void StateMachine::declare_parameters()
    {
        this->declare_parameter("source_type", params_.source_type);
        this->declare_parameter("action_type", params_.action_type);
        this->declare_parameter("nav_timeout", params_.nav_timeout);
        this->declare_parameter("align_timeout", params_.align_timeout);
        this->declare_parameter("gripper_timeout", params_.gripper_timeout);
        this->declare_parameter("delay_before_local_nav", params_.delay_before_local_nav);
        this->declare_parameter("delay_before_line_align", params_.delay_before_line_align);

        this->declare_parameter("topics.local_navigation_reached", params_.local_navigation_reached_topic);
        this->declare_parameter("topics.line_alignment_completed", params_.line_alignment_completed_topic);
        this->declare_parameter("topics.gripper_completed", params_.gripper_completed_topic);

        // Note: sub_node_params are loaded from YAML after the main parameters are known
    }

    void StateMachine::load_parameters()
    {
        params_.source_type = this->get_parameter("source_type").as_string();
        params_.action_type = this->get_parameter("action_type").as_string();
        params_.nav_timeout = this->get_parameter("nav_timeout").as_double();
        params_.align_timeout = this->get_parameter("align_timeout").as_double();
        params_.gripper_timeout = this->get_parameter("gripper_timeout").as_double();
        params_.delay_before_local_nav = this->get_parameter("delay_before_local_nav").as_double();
        params_.delay_before_line_align = this->get_parameter("delay_before_line_align").as_double();

        params_.local_navigation_reached_topic = this->get_parameter("topics.local_navigation_reached").as_string();
        params_.line_alignment_completed_topic = this->get_parameter("topics.line_alignment_completed").as_string();
        params_.gripper_completed_topic = this->get_parameter("topics.gripper_completed").as_string();


        // 从yaml配置文件加载子节点参数
        // 这种方法比as_string_map() 更健壮
        // 使用get_parameters获取所有参数，然后筛选
        // 这种方式假设sub_nodes是一个在参数服务器上的嵌套字典结构
        // yaml 文件的结构暗示了这一点，我们可以获取所有以sub_nodes. 开头的参数

        auto all_params = this->get_parameters({}); // 获取所有参数
        std::map<std::string, std::map<std::string, rclcpp::Parameter>> raw_sub_params; // 临时存储原始参数

        // 遍历所有参数
        for (const auto& param : all_params) {
            std::string name = param.get_name(); // 获取参数名
            // 检查参数名是否以 "sub_nodes." 开头
            if (name.rfind("sub_nodes.", 0) == 0) {
                // 移除 "sub_nodes." 前缀
                std::string full_sub_param_name = name.substr(10);
                // 查找节点名和参数名之间的分隔符 '.'
                size_t dot_pos = full_sub_param_name.find('.');
                if (dot_pos != std::string::npos) {
                    // 提取节点名 (例如: center_detection)
                    std::string node_name = full_sub_param_name.substr(0, dot_pos);
                    // 提取参数名 (例如: detection_mode)
                    std::string param_name = full_sub_param_name.substr(dot_pos + 1);
                    // 将参数对象存入临时 map，按节点名和参数名组织
                    raw_sub_params[node_name][param_name] = param;
                }
            }
        }

        // 处理原始参数：替换占位符，并存储为字符串 map
        for (const auto& [node_name, node_param_map] : raw_sub_params) {
            std::map<std::string, std::string> processed_params; // 存储处理后的参数 (名-值对)
            // 遍历当前节点的所有参数
            for (const auto& [param_name, rcl_param] : node_param_map) {
                // 将参数值转换为字符串表示形式 (例如，整数 42 变成 "42"，布尔值 true 变成 "true")
                std::string param_value_str = rcl_param.value_to_string();
                // 如果值原本是字符串，value_to_string() 可能会加上引号，这里尝试移除它们
                if (param_value_str.size() >= 2 && param_value_str.front() == '"' && param_value_str.back() == '"') {
                    param_value_str = param_value_str.substr(1, param_value_str.size() - 2);
                }
                // 执行字符串替换：将 "${source_type}" 替换为 params_.source_type 的值
                size_t pos;
                pos = param_value_str.find("${source_type}");
                if (pos != std::string::npos) {
                    param_value_str.replace(pos, 15, params_.source_type); // 15 是 "${source_type}" 的长度
                }
                // 执行字符串替换：将 "${action_type}" 替换为 params_.action_type 的值
                pos = param_value_str.find("${action_type}");
                if (pos != std::string::npos) {
                    param_value_str.replace(pos, 14, params_.action_type); // 14 是 "${action_type}" 的长度
                }
                // 将处理后的参数名和值存入临时 map
                processed_params[param_name] = param_value_str;
                RCLCPP_DEBUG(this->get_logger(), "  子节点 '%s' 参数 '%s' -> '%s'", node_name.c_str(), param_name.c_str(), param_value_str.c_str());
            }
            // 将处理好的参数 map 存入主参数结构中，以节点名作为键
            params_.sub_node_params[node_name] = processed_params;
        }


        // 记录最终处理好的子节点参数
        RCLCPP_INFO(this->get_logger(), "最终映射的子节点参数:");
        for (const auto& [node_name, params_map] : params_.sub_node_params) {
            RCLCPP_INFO(this->get_logger(), "  节点: %s", node_name.c_str());
            for (const auto& [param_name, param_value] : params_map) {
                RCLCPP_INFO(this->get_logger(), "    %s: %s", param_name.c_str(), param_value.c_str());
            }
        }
    }

    void StateMachine::create_lifecycle_clients()
    {
        // 使用LifecycleNodeInterface::create_client来获取客户端接口
        lifecycle_clients_["center_detection"] = rclcpp_lifecycle::LifecycleNodeInterface::create_client(
            this->shared_from_this(), "center_detection");
        lifecycle_clients_["local_navigation"] = rclcpp_lifecycle::LifecycleNodeInterface::create_client(
            this->shared_from_this(), "local_navigation");
        lifecycle_clients_["mqtt_cloud_sub"] = rclcpp_lifecycle::LifecycleNodeInterface::create_client(
            this->shared_from_this(), "mqtt_cloud_sub");
        lifecycle_clients_["line_alignment"] = rclcpp_lifecycle::LifecycleNodeInterface::create_client(
            this->shared_from_this(), "line_alignment");
        lifecycle_clients_["gripper_control"] = rclcpp_lifecycle::LifecycleNodeInterface::create_client(
            this->shared_from_this(), "gripper_control");
    }

    void StateMachine::create_parameter_clients()
    {
        param_clients_["center_detection"] = this->create_client<rcl_interfaces::srv::SetParameters>(
            "center_detection/set_parameters");
        param_clients_["local_navigation"] = this->create_client<rcl_interfaces::srv::SetParameters>(
            "local_navigation/set_parameters");
        param_clients_["mqtt_cloud_sub"] = this->create_client<rcl_interfaces::srv::SetParameters>(
            "mqtt_cloud_sub/set_parameters");
        param_clients_["line_alignment"] = this->create_client<rcl_interfaces::srv::SetParameters>(
            "line_alignment/set_parameters");
        param_clients_["gripper_control"] = this->create_client<rcl_interfaces::srv::SetParameters>(
            "gripper_control/set_parameters");
    }

    void StateMachine::configure_and_activate_node(const std::string& node_name)
    {
        auto client = lifecycle_clients_[node_name];
        if (!client) {
            RCLCPP_ERROR(this->get_logger(), "Lifecycle client for node '%s' not found!", node_name.c_str());
            return;
        }

        RCLCPP_INFO(this->get_logger(), "Configuring node: %s", node_name.c_str());
        auto goal = std::make_shared<lifecycle_msgs::srv::ChangeState::Request>();
        goal->transition.id = lifecycle_msgs::msg::Transition::TRANSITION_CONFIGURE;
        auto future = client->change_state(goal);
        auto future_result = future.wait_for(std::chrono::seconds(10)); // Timeout for configure call
        if(future_result == std::future_status::ready) {
            auto response = future.get();
            if(response.success) {
                RCLCPP_INFO(this->get_logger(), "Successfully configured node: %s", node_name.c_str());
            } else {
                RCLCPP_ERROR(this->get_logger(), "Failed to configure node: %s", node_name.c_str());
                // Optionally transition to ERROR state
                // current_state_ = State::ERROR;
                return;
            }
        } else {
            RCLCPP_ERROR(this->get_logger(), "Configure call for node '%s' timed out!", node_name.c_str());
            // Optionally transition to ERROR state
            // current_state_ = State::ERROR;
            return;
        }

        RCLCPP_INFO(this->get_logger(), "Activating node: %s", node_name.c_str());
        goal->transition.id = lifecycle_msgs::msg::Transition::TRANSITION_ACTIVATE;
        future = client->change_state(goal);
        future_result = future.wait_for(std::chrono::seconds(10)); // Timeout for activate call
        if(future_result == std::future_status::ready) {
            auto response = future.get();
            if(response.success) {
                RCLCPP_INFO(this->get_logger(), "Successfully activated node: %s", node_name.c_str());
            } else {
                RCLCPP_ERROR(this->get_logger(), "Failed to activate node: %s", node_name.c_str());
                // Optionally transition to ERROR state
                // current_state_ = State::ERROR;
                return;
            }
        } else {
            RCLCPP_ERROR(this->get_logger(), "Activate call for node '%s' timed out!", node_name.c_str());
            // Optionally transition to ERROR state
            // current_state_ = State::ERROR;
            return;
        }
    }

    void StateMachine::set_node_parameters(const std::string& node_name, const std::map<std::string, std::string>& params)
    {
        auto client = param_clients_[node_name];
        if (!client || !client->wait_for_service(std::chrono::seconds(1))) {
            RCLCPP_ERROR(this->get_logger(), "Parameter client for node '%s' not available or service not found!", node_name.c_str());
            return;
        }

        RCLCPP_INFO(this->get_logger(), "Setting parameters for node: %s", node_name.c_str());
        std::vector<rclcpp::Parameter> param_vec;
        for (const auto& [param_name, param_value_str] : params) {
            // Determine type based on string content or use a more robust method if needed
            // For simplicity, assume string type here. You might need to parse the value string.
            // For now, let's assume all values are strings as per YAML loading.
            // If sub-node expects int/float, you need to parse param_value_str accordingly.
            // For this example, we'll try to parse as double first, then string.
            double parsed_double;
            if (std::istringstream(param_value_str) >> parsed_double && std::istringstream(param_value_str).eof()) {
                 param_vec.push_back(rclcpp::Parameter(param_name, parsed_double));
            } else {
                 // Assume it's a string if not a number
                 param_vec.push_back(rclcpp::Parameter(param_name, param_value_str));
            }
        }

        auto request = std::make_shared<rcl_interfaces::srv::SetParameters::Request>();
        request->parameters.reserve(param_vec.size());
        for (const auto& param : param_vec) {
            request->parameters.push_back(param.to_parameter_msg());
        }

        auto future = client->async_send_request(request);
        auto future_result = future.wait_for(std::chrono::seconds(5)); // Timeout for set_parameters call
        if(future_result == std::future_status::ready) {
            auto response = future.get();
            for (size_t i = 0; i < response->results.size(); ++i) {
                const auto& result = response->results[i];
                const std::string& sent_param_name = response->parameters[i].name;
                if (!result.successful) {
                    RCLCPP_WARN(this->get_logger(), "Failed to set parameter '%s' on node '%s': %s", result.name.c_str(), node_name.c_str(), result.reason.c_str());
                }
            }
        } else {
            RCLCPP_ERROR(this->get_logger(), "SetParameters call for node '%s' timed out!", node_name.c_str());
        }
    }

    void StateMachine::state_machine_logic()
    {
        std::lock_guard<std::mutex> lock(state_mutex_); // Protect state transitions
        switch (current_state_) {
            case State::INIT:
                handle_init_state();
                break;
            case State::START_CENTER:
                handle_start_center_state();
                break;
            case State::WAIT_CENTER_DELAY:
                handle_wait_center_delay_state();
                break;
            case State::START_NAVIGATION:
                handle_start_navigation_state();
                break;
            case State::WAIT_NAV:
                handle_wait_nav_state();
                break;
            case State::START_MQTT:
                handle_start_mqtt_state();
                break;
            case State::WAIT_MQTT_DELAY:
                handle_wait_mqtt_delay_state();
                break;
            case State::START_ALIGNMENT:
                handle_start_alignment_state();
                break;
            case State::WAIT_ALIGN:
                handle_wait_align_state();
                break;
            case State::START_GRIPPER:
                handle_start_gripper_state();
                break;
            case State::WAIT_GRIPPER:
                handle_wait_gripper_state();
                break;
            case State::COMPLETED:
                handle_completed_state();
                break;
            case State::ERROR:
                // Handle error state if implemented
                break;
        }
    }

    void StateMachine::handle_init_state()
    {
        RCLCPP_INFO(this->get_logger(), "State: INIT");

        // Set parameters for all sub-nodes based on loaded config
        for (const auto& [node_name, params] : params_.sub_node_params) {
            set_node_parameters(node_name, params);
        }

        // Create subscriptions for completion topics
        nav_reached_sub_ = this->create_subscription<std_msgs::msg::Bool>(
            params_.local_navigation_reached_topic, 10,
            std::bind(&StateMachine::nav_reached_callback, this, std::placeholders::_1));

        align_completed_sub_ = this->create_subscription<std_msgs::msg::Bool>(
            params_.line_alignment_completed_topic, 10,
            std::bind(&StateMachine::align_completed_callback, this, std::placeholders::_1));

        gripper_completed_sub_ = this->create_subscription<std_msgs::msg::Bool>(
            params_.gripper_completed_topic, 10,
            std::bind(&StateMachine::gripper_completed_callback, this, std::placeholders::_1));

        RCLCPP_INFO(this->get_logger(), "Subscribed to completion topics.");

        // Transition to next state
        current_state_ = State::START_CENTER;
        RCLCPP_INFO(this->get_logger(), "Transitioning to START_CENTER state.");
    }

    void StateMachine::handle_start_center_state()
    {
        RCLCPP_INFO(this->get_logger(), "State: START_CENTER");
        configure_and_activate_node("center_detection");
        current_state_ = State::WAIT_CENTER_DELAY;

        // Start delay timer
        delay_timer_ = this->create_wall_timer(
            std::chrono::duration<double>(params_.delay_before_local_nav),
            std::bind(&StateMachine::delay_timer_callback, this));
        RCLCPP_INFO(this->get_logger(), "Started delay timer for %.2f seconds.", params_.delay_before_local_nav);
    }

    void StateMachine::handle_wait_center_delay_state()
    {
        RCLCPP_DEBUG(this->get_logger(), "State: WAIT_CENTER_DELAY");
        // Wait for delay_timer_ to expire
    }

    void StateMachine::handle_start_navigation_state()
    {
        RCLCPP_INFO(this->get_logger(), "State: START_NAVIGATION");
        configure_and_activate_node("local_navigation");
        current_state_ = State::WAIT_NAV;

        // Reset flag and start timeout timer
        nav_reached_ = false;
        timeout_timer_ = this->create_wall_timer(
            std::chrono::duration<double>(params_.nav_timeout),
            std::bind(&StateMachine::timeout_timer_callback, this));
        RCLCPP_INFO(this->get_logger(), "Started timeout timer for navigation (%.2f sec).", params_.nav_timeout);
    }

    void StateMachine::handle_wait_nav_state()
    {
        RCLCPP_DEBUG(this->get_logger(), "State: WAIT_NAV");
        if (nav_reached_) {
            RCLCPP_INFO(this->get_logger(), "Navigation reached signal received.");
            cancel_timers();
            current_state_ = State::START_MQTT;
            RCLCPP_INFO(this->get_logger(), "Transitioning to START_MQTT state.");
        }
        // Check for timeout handled by timeout_timer_callback
    }

    void StateMachine::handle_start_mqtt_state()
    {
        RCLCPP_INFO(this->get_logger(), "State: START_MQTT");
        configure_and_activate_node("mqtt_cloud_sub");
        current_state_ = State::WAIT_MQTT_DELAY;

        // Start delay timer
        delay_timer_ = this->create_wall_timer(
            std::chrono::duration<double>(params_.delay_before_line_align),
            std::bind(&StateMachine::delay_timer_callback, this));
        RCLCPP_INFO(this->get_logger(), "Started delay timer for %.2f seconds.", params_.delay_before_line_align);
    }

    void StateMachine::handle_wait_mqtt_delay_state()
    {
        RCLCPP_DEBUG(this->get_logger(), "State: WAIT_MQTT_DELAY");
        // Wait for delay_timer_ to expire
    }

    void StateMachine::handle_start_alignment_state()
    {
        RCLCPP_INFO(this->get_logger(), "State: START_ALIGNMENT");
        configure_and_activate_node("line_alignment");
        current_state_ = State::WAIT_ALIGN;

        // Reset flag and start timeout timer
        align_completed_ = false;
        timeout_timer_ = this->create_wall_timer(
            std::chrono::duration<double>(params_.align_timeout),
            std::bind(&StateMachine::timeout_timer_callback, this));
        RCLCPP_INFO(this->get_logger(), "Started timeout timer for alignment (%.2f sec).", params_.align_timeout);
    }

    void StateMachine::handle_wait_align_state()
    {
        RCLCPP_DEBUG(this->get_logger(), "State: WAIT_ALIGN");
        if (align_completed_) {
            RCLCPP_INFO(this->get_logger(), "Alignment completed signal received.");
            cancel_timers();
            current_state_ = State::START_GRIPPER;
            RCLCPP_INFO(this->get_logger(), "Transitioning to START_GRIPPER state.");
        }
        // Check for timeout handled by timeout_timer_callback
    }

    void StateMachine::handle_start_gripper_state()
    {
        RCLCPP_INFO(this->get_logger(), "State: START_GRIPPER");
        configure_and_activate_node("gripper_control");
        current_state_ = State::WAIT_GRIPPER;

        // Reset flag and start timeout timer
        gripper_completed_ = false;
        timeout_timer_ = this->create_wall_timer(
            std::chrono::duration<double>(params_.gripper_timeout),
            std::bind(&StateMachine::timeout_timer_callback, this));
        RCLCPP_INFO(this->get_logger(), "Started timeout timer for gripper (%.2f sec).", params_.gripper_timeout);
    }

    void StateMachine::handle_wait_gripper_state()
    {
        RCLCPP_DEBUG(this->get_logger(), "State: WAIT_GRIPPER");
        if (gripper_completed_) {
            RCLCPP_INFO(this->get_logger(), "Gripper completed signal received.");
            cancel_timers();
            current_state_ = State::COMPLETED;
            RCLCPP_INFO(this->get_logger(), "Transitioning to COMPLETED state.");
        }
        // Check for timeout handled by timeout_timer_callback
    }

    void StateMachine::handle_completed_state()
    {
        RCLCPP_INFO(this->get_logger(), "State: COMPLETED - Task finished successfully.");
        // Optionally, you could shut down or deactivate sub-nodes here if needed.
        // For now, just stay in COMPLETED state.
    }


    // --- Topic Callbacks ---
    void StateMachine::nav_reached_callback(const std_msgs::msg::Bool::SharedPtr msg)
    {
        if (msg->data) {
            std::lock_guard<std::mutex> lock(state_mutex_);
            nav_reached_ = true;
            RCLCPP_INFO(this->get_logger(), "Received navigation reached signal (true).");
        }
    }

    void StateMachine::align_completed_callback(const std_msgs::msg::Bool::SharedPtr msg)
    {
        if (msg->data) {
            std::lock_guard<std::mutex> lock(state_mutex_);
            align_completed_ = true;
            RCLCPP_INFO(this->get_logger(), "Received alignment completed signal (true).");
        }
    }

    void StateMachine::gripper_completed_callback(const std_msgs::msg::Bool::SharedPtr msg)
    {
        if (msg->data) {
            std::lock_guard<std::mutex> lock(state_mutex_);
            gripper_completed_ = true;
            RCLCPP_INFO(this->get_logger(), "Received gripper completed signal (true).");
        }
    }


    // --- Timer Callbacks ---
    void StateMachine::delay_timer_callback()
    {
        RCLCPP_INFO(this->get_logger(), "Delay timer expired.");
        delay_timer_->cancel();
        std::lock_guard<std::mutex> lock(state_mutex_);
        if (current_state_ == State::WAIT_CENTER_DELAY) {
            current_state_ = State::START_NAVIGATION;
            RCLCPP_INFO(this->get_logger(), "Transitioning to START_NAVIGATION state after delay.");
        } else if (current_state_ == State::WAIT_MQTT_DELAY) {
            current_state_ = State::START_ALIGNMENT;
            RCLCPP_INFO(this->get_logger(), "Transitioning to START_ALIGNMENT state after delay.");
        }
    }

    void StateMachine::timeout_timer_callback()
    {
        RCLCPP_WARN(this->get_logger(), "Timeout occurred in state %d.", static_cast<int>(current_state_));
        timeout_timer_->cancel();
        // For simplicity in this version, we just log the timeout.
        // In a more robust system, you might transition to an ERROR state or retry.
        // For now, we'll stay in the current state and wait for the completion flag or another timeout.
        // The main state machine logic will eventually handle the lack of progress.
        // Consider adding an error state transition here if needed.
    }

    void StateMachine::cancel_timers()
    {
        if (delay_timer_) {
            delay_timer_->cancel();
            delay_timer_.reset(); // Reset the shared_ptr
        }
        if (timeout_timer_) {
            timeout_timer_->cancel();
            timeout_timer_.reset();
        }
    }

} // namespace fangshe_sm


int main(int argc, char * argv[])
{
    rclcpp::init(argc, argv);
    rclcpp::spin(std::make_shared<fangshe_sm::StateMachine>());
    rclcpp::shutdown();
    return 0;
}